#pragma once
#include<vector>
#include <eigen3/Eigen/Core>
#include<eigen3/Eigen/Geometry>
#include<opencv2/core/eigen.hpp>
#include <opencv2/opencv.hpp>


constexpr auto RED = 1;
constexpr auto BLUE = 0;

using namespace cv;
using namespace std;
class PNP
{
private:
    bool _is_recorded; //写入视频判断参数
    int _color; //灯条颜色
    VideoWriter writer;
    Mat _camera_matrix; //相机内参矩阵
    Mat _distcoeffs; //相机畸变系数矩阵
    double _half_length, _half_width; //装甲板长宽度的一半
    vector<Point3f> _obj; //装甲板的世界坐标

    

    /**
     * @brief PNP姿态解算转角与测距
     * 
     * @param objectPoints 图像坐标系坐标
     * @return vector<double> result[0]--yaw轴转角、pitch轴转角、欧式距离
     */
    vector<double> p4psolution(const vector<Point2f> objectPoints) ;

public:
    PNP();

    /**
     * @brief 视频写入判断参数设置
     * 
     * @param is_recorded 是否进行图像处理后的效果视频录制
     */
    PNP(bool is_recorded);

    /**
     * @brief 视频写入判断参数及灯条颜色参数设置
     * 
     * @param is_recored 是否进行图像处理后的效果视频录制
     * @param color 视频中灯条的颜色
     */
    PNP(bool is_recorded, int color); //h或hpp头文件的各项函数和形参都只声明，不要定义或赋值，否则会有多次指定和声明的报错
    ~PNP();

    /**
     * @brief 装甲板区域和中心点图像的处理及标定
     * 
     * @param video_path 视频路径，暂时只做了装甲板_1.avi视频的处理
     * @return 运行信息 
     */
    int ImageprocessAndPnP(std::string video_path);

    /**
     * @brief 设定装甲板尺寸参数
     * 
     * @param length 装甲板长度
     * @param width 装甲板宽度
     */
    void set_objpoint(double length, double width);

    /**
     * @brief 设定相机参数和系数矩阵
     * 
     * @param camera_matrix 相机内参矩阵
     * @param distcoeffs 相机畸变系数矩阵
     */
    void set_camera_params(Mat camera_matrix, Mat distcoeffs);
    
};
